// ROS includes
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>

#include "ft_nodes/collision_check.h"

// c++ includes
#include <cstdlib>
#include <cstdio>
#include <fstream>

// OpenCV includes
//#include <cvblob/cvblob.h>
#include <opencv2/core/core.hpp>
#include <opencv2/contrib/contrib.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/core/types_c.h>

#include <boost/thread/mutex.hpp>

// standart c++ namespace
using namespace std;

// OpenCV namespace
using namespace cv;
using namespace cv_bridge;
//using namespace cvb;
        
// ROS namespace
namespace enc = sensor_msgs::image_encodings;


//-----------------------------------------------------------------------------

bool            g_bnHasDepth = false;
unsigned short  g_awDepth[ 640 * 480 ];


bool CheckCallback( ft_nodes::collision_checkRequest& request, ft_nodes::collision_checkResponse& response ) {
    
    if ( request.twist.twist.linear.x > 0.0 ) {
        
        bool bnSomethingNear = false;
        
        int nDepth;

        for ( int nRow = 220; nRow < 260 && !bnSomethingNear; nRow++ ) {
            
            for( int nCol = 240; nCol < 400; nCol++ ) {
                
                nDepth = g_awDepth[ nRow * 640 + nCol ];
                
                if ( nDepth > 0 && nDepth < 100 ) {
                    
                    ROS_INFO("Depth %d", nDepth);
                    bnSomethingNear = true;
                    break;
                }
            }
        }
        response.ack = !bnSomethingNear;
    }
    else {
        
        response.ack = true;
    }
}

void DepthMapCallback( const sensor_msgs::Image::ConstPtr& msg ) {

    cv_bridge::CvImagePtr cv_ptr;
    try {
        
      cv_ptr = cv_bridge::toCvCopy(msg, enc::TYPE_16UC1);
    }
    catch (cv_bridge::Exception& e ) {
        
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    
    char* depthData = (char*)cv_ptr->image.data;
    const int nTotalSize = 640*480;
    
    for (int i = 0, j = 0; j < nTotalSize; i+=2, j++) {

        unsigned short tmp = depthData[i + 1];
        tmp <<= 8;
        tmp += depthData[i];
        tmp &= 0xfff8;
        tmp >>= 3;
        g_awDepth[j]=tmp;
    }
    g_bnHasDepth = true;
}


//-----------------------------------------------------------------------------

int main(int argc, char **argv) {
    
    ros::init(argc, argv, "collision_avoidance");
    ros::NodeHandle n;
    
    ros::Subscriber    subD    = n.subscribe( "/camera/depth/image_raw", 2, DepthMapCallback );
    ros::ServiceServer service = n.advertiseService( "/collision_avoidance", CheckCallback    );
    
    ros::spin();
    
    return 0;	
}

